#include <iostream>
#include <math.h>

#include "arena_pole_cart.h"

using namespace std;

Arena_pole_cart::Arena_pole_cart()
{
  reset();

  // parameters
  _mass_cart = 10;
  _mass_pole = 2;
  _length_pole = 1;
  _g = 9.81; // gravity

  // arena
  objects = new Objects(200, 200);
  // cart
  Rectangle r(0, 180, 20, 10);
  objects->add(r);
  // pole
  Line l(100, 180, 100, 170 - _length_pole * 50);
  objects->add(l);
  update_arena();

}

Arena_pole_cart::~Arena_pole_cart()
{
}

void Arena_pole_cart::reset(void)
{
  Arena_base::reset();
  _theta = 0.1;
  _dtheta = 0;
  _position = 0;
  _dposition = 0;

  _F = 0;
}

double *Arena_pole_cart::get_theta_p(void)
{
  return &_theta;
}

double *Arena_pole_cart::get_cart_position_p(void)
{
  return &_position;
}

double *Arena_pole_cart::get_theta_velocity_p(void)
{
  return &_dtheta;
}

double *Arena_pole_cart::get_cart_velocity_p(void)
{
  return &_dposition;
}

void Arena_pole_cart::update_state(float time)
{
  double ddtheta;
  double ddposition;
  double dt = 0.01;

  double mass_total = _mass_pole + _mass_cart;

  while (_time < time) {
    if (_time + dt > time) {
      dt = time - _time;
    }
    double tmp = (-_F - _mass_pole * _length_pole * _dtheta * _dtheta * sin(
        _theta)) / mass_total;
    ddtheta = (_g * sin(_theta) + cos(_theta) * tmp) / (_length_pole * (4.0
        / 3.0 - (_mass_pole * cos(_theta) * cos(_theta)) / mass_total));
    ddposition = _F + _mass_pole * _length_pole * (_dtheta * _dtheta * sin(
        _theta) - ddtheta * cos(_theta)) / mass_total;

    _position += _dposition * dt;
    _dposition += ddposition * dt;
    _theta += _dtheta * dt;
    _dtheta += ddtheta * dt;

    //cout << _position << " " << _theta << endl;

    _time += dt;
  }
  if (fabs(_theta) > 1.5) {
    _failure = true;
  }
  update_arena();
}

void Arena_pole_cart::push(double force)
{
  _F = force;
}

boost::function<void(float)> Arena_pole_cart::push_function(void)
{
  return std::bind1st(std::mem_fun(&Arena_pole_cart::push), this);
}

void Arena_pole_cart::update(float time)
{
  if (time < _time) {
    cout << "ERROR: delta time negative" << endl;
    exit(1);
  }

  update_state(time);

  // remove forcing - will have to be re-applied from outside
  _F = 0;
}

void Arena_pole_cart::update_arena(void)
{
  // cart
  objects->rectangles[0].p1.x = 100 + _position * 10 - 10;
  // pole

  objects->lines[0].p1.x = 100 + _position * 10;
  objects->lines[0].p2.x = 100 + _position * 10 + sin(_theta) * 50
      * _length_pole;
  objects->lines[0].p2.y = 170 - cos(_theta) * 50 * _length_pole;
}

